#include "mainwindow.h"
#include <QApplication>

#include <ReflexxesAPI.h>
#include <RMLPositionFlags.h>
#include <RMLPositionInputParameters.h>
#include <RMLPositionOutputParameters.h>


#include <iostream>
#include <fstream>

#include "time.h"

using namespace std;

#define CYCLE_TIME_IN_SECONDS                   0.001
#define NUMBER_OF_DOFS                          2

static void testRml(void);

int main(int argc, char *argv[])
{
    testRml();

    return 0;

//    QApplication a(argc, argv);
//    MainWindow w;
//    w.show();

//    return a.exec();
}


static void testRml(void)
{
    double                      Time;

    int                         ResultValue                 =   0       ;

    ReflexxesAPI                *RML                        =   NULL    ;

    RMLPositionInputParameters  *IP                         =   NULL    ;

    RMLPositionOutputParameters *OP                         =   NULL    ;

    RMLPositionFlags            Flags                                   ;

    // ********************************************************************
    // Creating all relevant objects of the Type II Reflexxes Motion Library

    RML =   new ReflexxesAPI(                   NUMBER_OF_DOFS
                                            ,   CYCLE_TIME_IN_SECONDS   );

    IP  =   new RMLPositionInputParameters(     NUMBER_OF_DOFS          );

    OP  =   new RMLPositionOutputParameters(    NUMBER_OF_DOFS          );

    IP->CurrentPositionVector->VecData      [0] =    0.0      ;
    IP->CurrentPositionVector->VecData      [1] =    0.0      ;

    IP->CurrentVelocityVector->VecData      [0] =      0.0      ;
    IP->CurrentVelocityVector->VecData      [1] =      0.0      ;

    IP->CurrentAccelerationVector->VecData  [0] =      0.0      ;
    IP->CurrentAccelerationVector->VecData  [1] =      0.0      ;

    IP->MaxVelocityVector->VecData          [0] =    80.0      ;
    IP->MaxVelocityVector->VecData          [1] =    300.0      ;

    IP->MaxAccelerationVector->VecData      [0] =    8.0      ;
    IP->MaxAccelerationVector->VecData      [1] =    400.0      ;

    IP->MaxJerkVector->VecData              [0] =    50.0      ;
    IP->MaxJerkVector->VecData              [1] =    50.0      ;

    IP->TargetPositionVector->VecData       [0] =    1.0      ;
    IP->TargetPositionVector->VecData       [1] =    300.0      ;

    IP->TargetVelocityVector->VecData       [0] =     0.0       ;
    IP->TargetVelocityVector->VecData       [1] =     0.0       ;

    IP->SelectionVector->VecData            [0] =   true        ;
    IP->SelectionVector->VecData            [1] =   true        ;

    Flags.SynchronizationBehavior   =   RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION;

    clock_t start = clock();    // the code need to measure time

    // Calling the Reflexxes OTG algorithm
    ResultValue =   RML->RMLPosition(*IP, OP, Flags);

    double temp = OP->GetSynchronizationTime();

    double period = ceil(temp/CYCLE_TIME_IN_SECONDS);

    IP->MinimumSynchronizationTime = period * CYCLE_TIME_IN_SECONDS;

    ResultValue =   RML->RMLPosition(*IP, OP, Flags);

    double temp1 = OP->GetSynchronizationTime();

    clock_t end = clock();

    double timeSpend = ((double)(end - start)) / CLOCKS_PER_SEC;

    ofstream posFile("rml_pos.txt");
    ofstream velFile("rml_vel.txt");

    for(Time = 0; Time < temp1; Time += CYCLE_TIME_IN_SECONDS){

        RML->RMLPositionAtAGivenSampleTime(Time, OP);

        posFile << OP->NewPositionVector->VecData[0] << " "  << OP->NewPositionVector->VecData[1] << '\n';
        velFile << OP->NewVelocityVector->VecData[0] << " "  << OP->NewVelocityVector->VecData[1] << '\n';
    }

    posFile.close();
    velFile.close();

    return;
}
